import cv2
import numpy as np
import math

img = cv2.imread("pencil.png")

rows, cols, _ = img.shape

# Principle axis of object, defined relative to image
REL_AXIS = [(0.5, 0), (0.5, 1)]

# Principle axis in pixels
a_point1 = np.array((int(REL_AXIS[0][0] * cols), int(REL_AXIS[0][1] * rows)))
a_point2 = np.array((int(REL_AXIS[1][0] * cols), int(REL_AXIS[1][1] * rows)))
a_len = int(np.linalg.norm(a_point1 - a_point2))
a_direction_vec = a_point2 - a_point1
a_udir = a_direction_vec / a_len

def count_pixels(start, udirection):
    it = start
    count = 0

    while it[0] < cols and it[1] < rows and it[0] >= 0 and it[1] >= 0:
        pt = img[int(it[1]), int(it[0])]

        if np.all(pt == 0):
            break

        it = (it[0] + udirection[0], it[1] + udirection[1])
        count = count + 1

    return count

perp_dir = np.array([-a_udir[1], a_udir[0]])

perp_lens = []

for l in range(a_len):
    start = a_point1 + a_udir*l
    perp_lens += [count_pixels(start, perp_dir)]

depthImage = img.copy()

for y in range(rows):
    for x in range(cols):
        C = np.array((x, y))

        PAR = np.dot(a_udir, C)
        PERP = C - PAR * a_udir - a_point1

        depth = (perp_lens[int(PAR)] ** 2) - np.linalg.norm(PERP)**2

        if depth < 0:
            depth = 0

        depth = math.sqrt(depth)

        depthImage[y, x] = (depth * 4, depth * 4, depth * 4)

def make_target(angle):
    targetImage = np.zeros((rows * 3, rows * 3, 3), np.uint8)

    cost = math.cos(angle)
    sint = math.sin(angle)

    for y in range(rows):
        for x in range(cols):
            depth = int(depthImage[y, x][0])

            # Do 3D transform: matrix multiply or whatever
            (xP, yP, zP) = (x + rows, round(cost*y - sint*depth) + rows, depth)

            if depth > 0:
                targetImage[yP + 1, xP - 1] = img[y, x]
                targetImage[yP + 1, xP + 0] = img[y, x]
                targetImage[yP + 1, xP + 1] = img[y, x]
                targetImage[yP + 0, xP - 1] = img[y, x]
                targetImage[yP + 0, xP + 0] = img[y, x]
                targetImage[yP + 0, xP + 1] = img[y, x]
                targetImage[yP - 1, xP - 1] = img[y, x]
                targetImage[yP - 1, xP + 0] = img[y, x]
                targetImage[yP - 1, xP + 1] = img[y, x]

    return targetImage

# Create video

for angle in range(90):
    #cv2.waitKey(1)
    target = make_target(angle * math.pi / 180)
    #cv2.imshow("Target", target)
    cv2.imwrite("/dev/shm/" + str(100+angle)[1:] + ".bmp", target)
